#include <stdlib.h>
#include <stdio.h>
#include "support.h"
#include "uart.h"
#include "util/delay.h"
#include "pmd_control.h"
#include "pc_comm.h"
#include "twi_master_driver.h"
#include "lm89.h"
#include "spi_driver.h"

PMD_Control_t pmd;

PC_Control_t pc;

TWI_Master_t temp_sensor;

SPI_Slave_t medulla_spi = {NULL, NULL};

volatile uint8_t medulla_spi_data[10];
volatile uint8_t medulla_spi_data_count = 0;
volatile uint8_t medulla_flag = 0;
volatile uint8_t medulla_send_buffer[10];





volatile int temp = 0;

volatile uint8_t estop_enable = ESTOP_DEFAULT;
volatile uint8_t estop_flag = 0;


int main(void){

	//	clock_32mhz_init();

	clock_12mhz_external_init();

	PORTA.DIR |= 0b10000011;
	medulla_send_buffer[0] = 0x56;
	//For pmd_spi sends
	//pmd SPI stuff
	uint8_t pmd_spi_data[2] = {0,0};

	//gernal purpose
	int i;
	uint8_t check_sum;

	//Estop Enable
	uint8_t bus_voltage_estop_enable = 1;

	//Buffer for SPI Command Scaling
	uint16_t spi_motor_command_buffer = 0;


	uint8_t string[30];
	uint8_t self_clock = 0;
	//Enable interrupts
	PMIC.CTRL |= PMIC_LOLVLEX_bm | PMIC_MEDLVLEX_bm | PMIC_HILVLEX_bm | 
		PMIC_LOLVLEN_bm | PMIC_MEDLVLEN_bm | PMIC_HILVLEN_bm; 
	sei();

	//PORTA.DIR |= 0b10000011;
	PORTA.OUT = 0;

	//Set up PORTA ESTOP input for PIN6 risting edge
	PORTA.INTCTRL = 0b00000011;
	PORTA.INT0MASK = (1<<6);
	PORTA.PIN6CTRL = 0b00000001;

	//Set Tx pin to output
	PORTE.DIR |= 0x08;
	PORTB.DIR |= 3;

	//Make sure it resets
	PORTB.OUT &= ~1;

	//PMD SPI stuff
	//Set TX and XCK to output
	PORTD.DIR |= (1<<1) | (1<<3);
	uart_spi_init();

	SPI_SlaveInit(&medulla_spi,
			&SPIC,
			&PORTC,
			false,
			SPI_MODE_0_gc,
			SPI_INTLVL_MED_gc);
	_delay_ms(200);


	PORTB.OUT |= 1;

	//pmd_init(&pmd, &USARTC0, 576);
	_delay_ms(2000);
	pc_init(&pc, &USARTE0, 1152, &pmd);
	//pmd_set_baud(&pmd, 4608);

	//pc_set_pmd(&pc, &pmd);
	/*
	   set_motor_mode(&pmd, 0);
	   set_commutation_mode(&pmd, 1);
	   set_condition_mask(&pmd, 2, 0);
	   set_signal_sense(&pmd, 0);
	   set_dead_time(&pmd, deadtime);
	   set_pwm_sense(&pmd, 0b00111111);

	   set_phase_counts(&pmd, 24);
	 */

	twiInitiate(&temp_sensor,&TWIE);

	_delay_ms(500);
	//PMD Enable
	PORTD.DIR |= 0b00100000;
	//PMD Manual Fix Disable
	//PORTD.DIR &= ~0b00100000;


	//Initialize Temperature Sensor
	//get_temp((int*)&temp, &temp_sensor);

	//Timer Counter 0 set to clock div 1024 (assuming clock is 12Mhz for this
	TCC0.CTRLA = 0b00000100;
	TCC0.CTRLB = 0;
	//Roughly 1ms period
	TCC0.PER = 1500;

	//Set Current Offsets
/*
	get_current_A(&pmd);
	while(!(pmd.data_available));

	pmd.data_available = 0;

	pc.current_A = (pmd.data_in[2] << 8) | (pmd.data_in[3] & 0xff);
	get_current_B(&pmd);
	while(!(pmd.data_available));

	pmd.data_available = 0;

	pc.current_B = (pmd.data_in[2] << 8) | (pmd.data_in[3] & 0xff);

	set_analog_offset(&pmd, CURRENT_A, -pc.current_A);
	set_analog_offset(&pmd, CURRENT_B, -pc.current_B);
*/


	while(1){
		if(medulla_flag){
			medulla_flag = 0;
			medulla_spi_data_count = 0;

			//Verify Checksum
			check_sum = 0;
			for(i=0;i<9;i++){
				check_sum += medulla_spi_data[i];
			}
			if(check_sum == 0){

				//Check command byte
				//general command packet
				if(medulla_spi_data[2] == 'c'){
					pc.spi_motor_command = (medulla_spi_data[4] << 8) | (medulla_spi_data[5] & 0xff);
					spi_motor_command_buffer = (uint16_t)(pc.spi_motor_command)+ (uint16_t)(1<<15);
					pmd_spi_data[0] = (uint8_t)((spi_motor_command_buffer >> 8) & 0xff);
					pmd_spi_data[1] = (uint8_t)(spi_motor_command_buffer & 0xff);

					for(i=0;i<2;i++){
						while(!(USARTD0.STATUS & (1<<5)));
						USARTD0.DATA = pmd_spi_data[i];
					}

					//Get requested Telemetry
					//For Phase A Current
					if(medulla_spi_data[3] == 'a'){
						get_current_A(&pmd);
						while(!pmd.data_available);
						medulla_send_buffer[7] = pmd.data_in[2];
						medulla_send_buffer[8] = pmd.data_in[3];
					}
				}


				//Compulsery Stuff
				//Update Vbus and Check E-Stop
				//get_bus_voltage(&pmd);
				//while(!pmd.data_available);
				pmd.data_available = 0;
				pc.bus_voltage = (pmd.data_in[2] << 8) |
					(pmd.data_in[3] & 0xff);

				//get_temp((int*)&temp, &temp_sensor);
				pc.temp = temp;
				medulla_send_buffer[5] = pc.temp + 55;
				medulla_send_buffer[6] = 0;

				if(pc.temp > pc.temp_max){
					trigger_estop((uint8_t*)&estop_flag);
					pc.status |= ESTOPPED;

				}
				if(bus_voltage_estop_enable){
					if(pc.bus_voltage > pc.bus_voltage_upper_limit){
						trigger_estop((uint8_t*)&estop_flag);
						pc.status |= ESTOPPED;

					}
					else if(pc.bus_voltage < pc.bus_voltage_lower_limit){
						trigger_estop((uint8_t*)&estop_flag);
						pc.status |= ESTOPPED;


					}
				}
				medulla_send_buffer[3] = pc.bus_voltage >> 8;
				medulla_send_buffer[4] = pc.bus_voltage & 0xff;
			}

			medulla_send_buffer[2] = 0;
			if(pc.status == ESTOPPED){
				medulla_send_buffer[2] |= ESTOPPED;
			}

			//generate checksum
			check_sum = 0;
			for(i=0;i<9;i++){
				check_sum += medulla_send_buffer[i];
			}
			medulla_send_buffer[1] = 256-check_sum;

		}

		//For if bus voltage and temp should be polled without help from medulla
		if(self_clock){
			if(TCC0.INTFLAGS & 0x01){
				TCC0.INTFLAGS = 1;

				//PORTA.OUT ^=3;


				//Update Vbus and Check E-Stop
				//get_bus_voltage(&pmd);
				//while(!pmd.data_available);
				pmd.data_available = 0;
				//pc.bus_voltage = (pmd.data_in[2] << 8) |
				//	(pmd.data_in[3] & 0xff);

				get_temp((int*)&temp, &temp_sensor);
				pc.temp = temp;

				if(pc.temp > pc.temp_max){
					trigger_estop((uint8_t*)&estop_flag);
					pc.status |= ESTOPPED;

				}
				if(bus_voltage_estop_enable){
					/*
					if(pc.bus_voltage > pc.bus_voltage_upper_limit){
						trigger_estop((uint8_t*)&estop_flag);
						pc.status |= ESTOPPED;

					}
					else if(pc.bus_voltage < pc.bus_voltage_lower_limit){
						trigger_estop((uint8_t*)&estop_flag);
						pc.status |= ESTOPPED;


					}
					*/
				}
				//Write to SPI, offset by 0x8000 (16bit)

				spi_motor_command_buffer = (uint16_t)((pc.spi_motor_command) + (uint16_t)(1<<15));
				pmd_spi_data[0] = (uint8_t)((spi_motor_command_buffer >> 8) & 0xff);
				pmd_spi_data[1] = (uint8_t)(spi_motor_command_buffer & 0xff);

				for(i=0;i<2;i++){
					while(!(USARTD0.STATUS & (1<<5)));
					USARTD0.DATA = pmd_spi_data[i];
				}




			}
		}
		if(pc.data_available){
			pc.data_available = 0;

			if(pc.serial_mode){
				if(pc.serial_flag){
					pc.data_in_count = 0;
					pc.serial_flag = 0;
					sprintf((char*)string, "I am now in serial mode\n\r");
					sendstring(&(pc.uart), string);
				}
				else if(pc.data_in_count == 0){
					if(pc.data_in[0] == 'r'){
						//sprintf((char*)string, "I'm Here!\n\r");
						//sendstring(&serial, string);
						get_motor_mode(&pmd);

					}
					else if(pc.data_in[0] == '?'){
						sendchar(&(pc.uart), pc.temp + 55);
						if(estop_flag){
							sendchar(&(pc.uart), 'E');
						}
						else{
							sendchar(&(pc.uart), 'N');
						}
					}
					else if(pc.data_in[0] == 'l'){
						self_clock ^= 1;
						if(self_clock){
							sprintf((char*)string, "I am now polling for bus voltage and temperature data\n\r");
						}
						else{
							sprintf((char*)string, "I am no longer polling for bus voltage and temperature\n\r");
						}
						sendstring(&(pc.uart), string);
					}
					else if(pc.data_in[0] == 'p'){
						estop_enable ^= 1;
						if(estop_enable){
							sprintf((char*)string, "External Estop Enabled\n\r");
						}
						else{
							sprintf((char*)string, "External Estop Disabled\n\r");
						}
						sendstring(&(pc.uart), string);
					}
					else if(pc.data_in[0] == '-'){
						pc.temp_max -= 5;
						sprintf((char*)string, "Temp Max: %d\n\r", pc.temp_max);
						sendstring(&(pc.uart), string);
					}
					else if(pc.data_in[0] == '+'){
						pc.temp_max += 5;
						sprintf((char*)string, "Temp Max: %d\n\r", pc.temp_max);

						sendstring(&(pc.uart), string);
					}



					else if(pc.data_in[0] == 'x'){
						pc.commutation_mode ^= 1;
						pc.commutation_mode &= 1;
						set_commutation_mode(&pmd, pc.commutation_mode);
						sprintf((char*)string, "Commutation Mode: %d\n\r", pc.commutation_mode);
						sendstring(&(pc.uart), string);

					}
					else if(pc.data_in[0] == 'b'){
						//get_bus_voltage(&pmd);
						//while(!pmd.data_available);
						pmd.data_available = 0;
						pc.bus_voltage = (pmd.data_in[2] << 8) | (pmd.data_in[3] & 0xff);
						sprintf((char*)string, "Bus Voltage: %d\n\r", pc.bus_voltage);
						sendstring(&(pc.uart), string);

					}
					else if(pc.data_in[0] == 't'){
						get_motor_command(&pmd);
					}
					else if(pc.data_in[0] == 'u'){
						pc.motor_command += 128;
						set_motor_command(&pmd, pc.motor_command);
					}
					else if(pc.data_in[0] == 'j'){
						pc.motor_command -= 128;
						set_motor_command(&pmd, pc.motor_command);
					}
					else if(pc.data_in[0] == 'f'){
						get_motor_limit(&pmd);
					}
					else if(pc.data_in[0] == 'i'){
						pc.motor_limit += 128;
						set_motor_limit(&pmd, pc.motor_limit);
					}
					else if(pc.data_in[0] == 'k'){
						pc.motor_limit -= 128;
						set_motor_limit(&pmd, pc.motor_limit);
					}
					else if(pc.data_in[0] == 'd'){
						get_pwm_frequency(&pmd);
					}
					else if(pc.data_in[0] == 'c'){
						get_phase_counts(&pmd);
					}
					else if(pc.data_in[0] == '0'){
						pc.phase_counts ++;
						if(pc.phase_counts > 10000){
							pc.phase_counts = 10000;
						}
						set_phase_counts(&pmd, pc.phase_counts);
						sprintf((char*)string, "Phase Counts:%d\n\r", pc.phase_counts);
						sendstring(&(pc.uart), string);

					}
					else if(pc.data_in[0] == '9'){
						pc.phase_counts --;
						if(pc.phase_counts < 1){
							pc.phase_counts = 1;
						}
						set_phase_counts(&pmd, pc.phase_counts);
						sprintf((char*)string, "Phase Counts:%d\n\r", pc.phase_counts);
						sendstring(&(pc.uart), string);

					}
					else if(pc.data_in[0] == 'm'){
						get_commutation_mode(&pmd);
					}
					else if(pc.data_in[0] == 'a'){
						get_dead_time(&pmd);
					}
					else if(pc.data_in[0] == 'z'){
						get_pwm_limit(&pmd);
					}
					else if(pc.data_in[0] == 's'){
						get_activity_status(&pmd);
					}
					else if(pc.data_in[0] == 'e'){
						pc.motor_armed ^= 1;
						if(!estop_flag){
							PORTD.OUT ^= 0b00100000;
						}
						else{
							PORTD.OUT &= ~0b00100000;
						}
						if(PORTD.OUT & 0b00100000){
							sprintf((char*)string, "I'm Armed, Beware\n\r");
						}
						else{
							sprintf((char*)string, "I'm Not Armed\n\r");
						}
						sendstring(&(pc.uart), string);
					}
					else if(pc.data_in[0] == '['){
						if(pc.dead_time > 0){
							pc.dead_time --;
						}
						set_dead_time(&pmd, pc.dead_time);
						sprintf((char*)string, "Deadtime is now: %d\n\r", pc.dead_time);
						sendstring(&(pc.uart), string);


					}
					else if(pc.data_in[0] == ']'){
						if(pc.dead_time < 55){
							pc.dead_time ++;
						}
						set_dead_time(&pmd, pc.dead_time);
						sprintf((char*)string, "Deadtime is now: %d\n\r", pc.dead_time);
						sendstring(&(pc.uart), string);
					}

					else if(pc.data_in[0] == ','){
						//get_temp((int*)&temp, &temp_sensor);
						sprintf((char*)string, "Temp: %d\n\r", temp);
						sendstring(&(pc.uart), string);
					}
					else if(pc.data_in[0] == '~'){
						pc.data_in_count ++;
						sprintf((char*)string, "Awaiting Input\n\r");
						sendstring(&(pc.uart), string);

					}


				}
				else{
					if((pc.data_in[1] == 'e') && (pc.data_in_count == 1)){
						pc.data_in_count ++;
					}
					else if((pc.data_in[2] == 'x') && (pc.data_in_count == 2)){
						pc.data_in_count ++;
					}
					else if((pc.data_in[3] == 'i') && (pc.data_in_count == 3)){
						pc.data_in_count ++;
					}
					else if((pc.data_in[4] == 't') & (pc.data_in_count == 4)){
						pc.serial_mode = 0;
						pc.data_in_count = 0;
						sprintf((char*)string, "Entering Packet Mode\n\r");
						sendstring(&(pc.uart), string);
					}

				}




			}

			else{
				pc.data_in_count = 0;
				if(pc.data_in[4] == 'S'){
					pc.motor_mode = pc.data_in[6];
					pc.commutation_mode = pc.data_in[7];
					pc.loop_mode = pc.data_in[8];
					pc.dead_time = pc.data_in[9];
					pc.motor_limit = (pc.data_in[10] << 8) |(pc.data_in[11] & 0xff);
					pc.phase_counts = (pc.data_in[12] << 8) | (pc.data_in[13] & 0xff);
					pc.velocity_scalar = (pc.data_in[14] << 8) | (pc.data_in[15] & 0xff);
					pc.motor_accelleration = (uint32_t)(((uint32_t)pc.data_in[16] << 24) | 
							((uint32_t)pc.data_in[17] << 16) | 
							((uint32_t)pc.data_in[18] << 8) | 
							((uint32_t)pc.data_in[19] & 0xff));
					pc.velocity_loop_gain_kp = (pc.data_in[20] << 8) | 
						(pc.data_in[21] & 0xff);
					pc.velocity_loop_gain_ki = (pc.data_in[22] << 8) | 
						(pc.data_in[23] & 0xff);
					pc.velocity_loop_gain_limit = (pc.data_in[24] << 8) | 
						(pc.data_in[25] & 0xff);
					pc.current_loop_gain_kp = (pc.data_in[26] << 8) | (pc.data_in[27] & 0xff);
					pc.current_loop_gain_ki = (pc.data_in[28] << 8) | (pc.data_in[29] & 0xff);
					pc.current_loop_gain_limit = (pc.data_in[30] << 8) | (pc.data_in[31] & 0xff);
					pc.temp_max = pc.data_in[32] - 55;
					pc.command_source = pc.data_in[33];

					//Be aware, pc_set_pmd will disarm pmd
					//pc_set_pmd(&pc, &pmd);
					pc_acknowledge(&pc);
				}
				else if(pc.data_in[4] == 'C'){
					//if motor loop is on, assuming velocity
					if(!pc.motor_mode){
						pc.motor_command = (pc.data_in[6] << 8) | (pc.data_in[7] & 0xff);
					//	set_motor_command(&pmd, pc.motor_command);
					}
					pc_acknowledge(&pc);
				}
				else if(pc.data_in[4] == 'c'){
					//get_motor_command(&pmd);
					//while(!(pmd.data_available));
					pmd.data_available = 0;

					pc.motor_command = (pmd.data_in[2] << 8) | (pmd.data_in[3] & 0xff);
					
					pc_give_command(&pc);
				}
				else if(pc.data_in[4] == 'V'){
					pc.motor_velocity = ((uint32_t)pc.data_in[6] << 24) | ((uint32_t)pc.data_in[7] << 16) | (pc.data_in[8] << 8) | (pc.data_in[9] & 0xff);
					//set_velocity(&pmd, pc.motor_velocity);

					pc_acknowledge(&pc);
				}
				else if(pc.data_in[4] == 'v'){
					//get_velocity(&pmd);
					//while(!(pmd.data_available));
					pmd.data_available = 0;

					pc.motor_velocity = ((uint32_t)pmd.data_in[2] << 24) | ((uint32_t)pmd.data_in[3] << 16) | (pmd.data_in[4] << 8) | (pmd.data_in[5] & 0xff);
					;
					pc_give_velocity(&pc);
				}

				else if(pc.data_in[4] == 'E'){
					pc.motor_armed = pc.data_in[6];
					if((pc.motor_armed) && (!estop_flag)){
						PORTD.OUT |= 0b00100000;
					}
					else{
						PORTD.OUT &= ~0b00100000;
					}
					pc_acknowledge(&pc);

				}
				//Set to 'L'ook out for self by taking voltage and temp reads
				else if(pc.data_in[4] == 'L'){
					self_clock = pc.data_in[6];
					pc_acknowledge(&pc);
				}
				//Demand Acknowledgment
				else if(pc.data_in[4] == 'A'){

					//get_bus_voltage(&pmd);
					//while(!pmd.data_available);
					pmd.data_available = 0;
					pc.bus_voltage = (pmd.data_in[2] << 8) |
						(pmd.data_in[3] & 0xff);

					get_temp((int*)&temp, &temp_sensor);
					pc.temp = temp;
					pc_acknowledge(&pc);

				}
				//Trigger ESTOP
				else if(pc.data_in[4] == 'K'){
					//PORTA.OUT ^= 3;
					trigger_estop((uint8_t*)&estop_flag);
					pc.status |= ESTOPPED;
					pc_acknowledge(&pc);

				}
				else if(pc.data_in[4] == 'X'){
					pc.spi_motor_command = (uint16_t)((int32_t)((pc.data_in[6] << 8) | (pc.data_in[7] & 0xff)));
					pc_acknowledge(&pc);
				}
				else if(pc.data_in[4] == 'x'){
					pc_give_spi(&pc);
				}
				else if(pc.data_in[4] == 'i'){
					//get_current_A(&pmd);
					//while(!(pmd.data_available));

					pmd.data_available = 0;

					pc.current_A = (pmd.data_in[2] << 8) | (pmd.data_in[3] & 0xff);
					//get_current_B(&pmd);
					//while(!(pmd.data_available));

					pmd.data_available = 0;

					pc.current_B = (pmd.data_in[2] << 8) | (pmd.data_in[3] & 0xff);

					pc_give_current(&pc);
				}
				else if(pc.data_in[4] == 'd'){
					//get_loop_error(&pmd, CURRENT_A);
					//while(!(pmd.data_available));

					pmd.data_available = 0;

					pc.error_A = (pmd.data_in[4] << 8) | (pmd.data_in[5] & 0xff);

					//get_loop_error(&pmd, CURRENT_B);
					//while(!(pmd.data_available));

					pmd.data_available = 0;

					pc.error_B = (pmd.data_in[4] << 8) | (pmd.data_in[5] & 0xff);

					pc_give_error(&pc);
				}
				else if(pc.data_in[4] == 'D'){
					//set_analog_offset(&pmd, CURRENT_A, -pc.current_A);
					//set_analog_offset(&pmd, CURRENT_B, -pc.current_B);
					pc_acknowledge(&pc);

				}

				else if(pc.data_in[4] == 'z'){
					//get_event_status(&pmd);
					//while(!(pmd.data_available));

					pmd.data_available = 0;

					pc.event_status = (pmd.data_in[2] << 8) | (pmd.data_in[3] & 0xff);
					pc_give_event_status(&pc);
				}
				else if(pc.data_in[4] == 'l'){
					//get_loop_command(&pmd, CURRENT_A);
					//while(!(pmd.data_available));

					pmd.data_available = 0;

					pc.current_command_A = (pmd.data_in[4] << 8) | (pmd.data_in[5] & 0xff);

					//get_loop_command(&pmd, CURRENT_B);
					//while(!(pmd.data_available));

					pmd.data_available = 0;

					pc.current_command_B = (pmd.data_in[4] << 8) | (pmd.data_in[5] & 0xff);

					pc_give_current_command(&pc);
				}


			}
		}

		if(pc.data_read_request){
			pc.data_read_request = 0;
			pc.data_in_count = 0;

			//get_motor_mode(&pmd);
			//while(!(pmd.data_available));
			pmd.data_available = 0;
			pc.motor_mode = (pmd.data_in[2] << 8) | (pmd.data_in[3] & 0xff);


			//get_loop_mode(&pmd);
			//while(!(pmd.data_available));
			pmd.data_available = 0;
			if(!generate_checksum(pmd.data_in, 4)){
				pc.loop_mode = (pmd.data_in[2] << 8) | (pmd.data_in[3] & 0xff);
				PORTA.OUT ^= 3;
			}


			//get_loop_gain(&pmd, LOOP_CURRENT_KP);
			//while(!(pmd.data_available));
			pmd.data_available = 0;
			if(!generate_checksum(pmd.data_in, 6)){
				pc.current_loop_gain_kp = (pmd.data_in[4] << 8) | (pmd.data_in[5] & 0xff);
			}

			//get_loop_gain(&pmd, LOOP_CURRENT_KI);
			//while(!(pmd.data_available));
			pmd.data_available = 0;
			pc.current_loop_gain_ki = (pmd.data_in[4] << 8) | (pmd.data_in[5] & 0xff);


			//get_loop_gain(&pmd, LOOP_CURRENT_LIMIT);
			//while(!(pmd.data_available));
			pmd.data_available = 0;
			pc.current_loop_gain_limit = (pmd.data_in[4] << 8) | (pmd.data_in[5] & 0xff);



			send_pc_update(&pc);
		}

		//	get_motor_mode(&pmd);
		if(pmd.data_available){

			pmd.data_available = 0;

			if(!(generate_checksum(pmd.data_in, pmd.data_in_count + 1))){

				if(pmd.command == GET_MOTOR_MODE){
					pc.motor_mode = pc.data_in[3];
					if(pc.serial_mode == 1){
						sprintf((char*)string, "Motor Mode: %d\n\r", pmd.data_in[3]);
						sendstring(&(pc.uart), string);


					}
				}

				else if(pmd.command == GET_MOTOR_COMMAND){
					pc.motor_command = (pmd.data_in[2] << 8) | (pmd.data_in[3] & 0xff);
					if(pc.serial_mode == 1){
						sprintf((char*)string, "Motor Command: %d\n\r", (int)((pmd.data_in[2] << 8) | (pmd.data_in[3] & 0xff)));
						sendstring(&(pc.uart), string);

					}
				}
				else if(pmd.command == GET_MOTOR_LIMIT){
					pc.motor_limit = (pmd.data_in[2] << 8) | (pmd.data_in[3] & 0xff);
					if(pc.serial_mode == 1){
						sprintf((char*)string, "Motor Limit: %d\n\r", (int)((pmd.data_in[2] << 8) | (pmd.data_in[3] & 0xff)));
						sendstring(&(pc.uart), string);

					}
				}
				else if(pmd.command == GET_PWM_FREQUENCY){
					if(pc.serial_mode == 1){
						sprintf((char*)string, "PWM Frequency: %d\n\r", (int)((pmd.data_in[2] << 8) | (pmd.data_in[3] & 0xff)));
						sendstring(&(pc.uart), string);
					}

				}
				else if(pmd.command == GET_COMMUTATION_MODE){
					pc.commutation_mode = pmd.data_in[3];
					if(pc.serial_mode == 1){
						sprintf((char*)string, "Commutation Mode: %d\n\r", (int)((pmd.data_in[2] << 8) | (pmd.data_in[3] & 0xff)));
						sendstring(&(pc.uart), string);

					}
				}
				else if(pmd.command == GET_PHASE_COUNTS){
					if(pc.serial_mode == 1){
						sprintf((char*)string, "Phase Counts: %d\n\r", (int)((pmd.data_in[2] << 8) | (pmd.data_in[3] & 0xff)));
						sendstring(&(pc.uart), string);
					}

				}
				else if(pmd.command == GET_DEAD_TIME){
					pc.dead_time = pmd.data_in[3];
					if(pc.serial_mode == 1){
						sprintf((char*)string, "Dead Time: %d\n\r", (int)((pmd.data_in[2] << 8) | (pmd.data_in[3] & 0xff)));
						sendstring(&(pc.uart), string);

					}
				}
				else if(pmd.command == GET_PWM_LIMIT){
					//pc.pwm_limit = (pmd.data_in[2] << 8) | (pmd.data_in[3] & 0xff);
					if(pc.serial_mode == 1){
						sprintf((char*)string, "PWM Limit: %d\n\r", (int)((pmd.data_in[2] << 8) | (pmd.data_in[3] & 0xff)));
						sendstring(&(pc.uart), string);
					}

				}
				else if(pmd.command == GET_ACTIVITY_STATUS){
					if(pc.serial_mode == 1){
						sprintBinary(string, (int*)&pmd.data_in[2]);
						sendstring(&(pc.uart), string);
					}

				} 
			}
			pmd.data_in_count = 0;

		}
		if(pmd.error){
			pmd.error = 0;
			//should add stuff here
			pmd.data_in_count = 0;
			if(pc.serial_mode){
				sprintf((char*)string, "Error %d\n\r", pmd.data_in[0]);
				sendstring(&(pc.uart), string);
			}

		}


	}


	return 0;

}
/*Usart module interrupt to inform data has been properly sent*/
ISR(USARTE0_DRE_vect){
	USART_DataRegEmpty(&(pc.uart));
}
        /*Xbee read interrupt*/
ISR(USARTE0_RXC_vect){
	pc_handle(&pc);

}

ISR(USARTC0_DRE_vect){

	USART_DataRegEmpty(&(pmd.uart));
}
        /*Xbee read interrupt*/
ISR(USARTC0_RXC_vect){
	//USART_RXComplete(&(pmd.uart));
	//pmd.data_in[pmd.data_in_count] = USART_RXBuffer_GetByte(&(pmd.uart));
	//
	pmd_in_handle(&pmd);

}
ISR(TWIE_TWIM_vect){
        TWI_MasterInterruptHandler(&temp_sensor);
}
ISR(SPIC_INT_vect)
{
	/* Get received data. */
	medulla_spi_data[medulla_spi_data_count] = SPI_SlaveReadByte(&medulla_spi);
	/*
	if(medulla_spi_data[medulla_spi_data_count] == 0x56){
		medulla_spi_data_count = 0;
	}
	*/
	SPI_SlaveWriteByte(&medulla_spi, medulla_send_buffer[medulla_spi_data_count]);
	medulla_spi_data_count ++;
	if(medulla_spi_data_count == 8){
		medulla_spi_data_count = 0;
		medulla_flag = 1;
	}
}
ISR(PORTA_INT0_vect){
	if(estop_enable){
		trigger_estop((uint8_t*)&estop_flag);
		pc.status |= ESTOPPED;
	}
}


